#include "MoveSenseCamera.h"
#include "MoveSenseFactory.h"
#include "Device.h"
#include "CameraRegister.h"
#include "ConfigFile.h"


namespace movesense {

//Camera Factory
MoveSenseFactory moveSenseFactory;
Device device;


MoveSenseCamera::MoveSenseCamera( int index/*=0*/ )
{
	int total = device.ScanCameras();
	if (total <= index)
	{
		camera = NULL;
		return ;
	}

	ConfigFile configFile(device.cameraInfoList);

	total = configFile.cameraList.size();
	if (total <= index)
	{
		camera = NULL;
		return;
	}

	camera = moveSenseFactory.GetCamera(configFile.cameraList[index]);

	setting = &(camera->info.setting);

	algorithm = NULL;
}

MoveSenseCamera::~MoveSenseCamera()
{
	delete camera;
}

bool MoveSenseCamera::IsAvailable()
{
	if (camera)
	{
		return true;
	}

	return false;
}

int MoveSenseCamera::Open()
{
	if (!IsAvailable())
	{
		return MS_NO_DEVICE;
	}

	Resolution resLR = setting->resolution;
	Resolution resD = resLR;
	int bytesD = setting->disparity.bpp / 8;

	Resolution resT; //transfer

	switch (setting->mode)
	{
	case CAMERA_LR:
	case CAMERA_LR_HD:
		imageDataLength = resLR.width*resLR.height*2;
		break;

	case CAMERA_LD:
	case CAMERA_LD_HD:
		imageDataLength = resLR.width*resLR.height + resD.width*bytesD*resD.height;
		break;

	case CAMERA_LRD:
	case CAMERA_LRD_HD:
		imageDataLength = resLR.width*resLR.height*2 + resD.width*bytesD*resD.height;
		break;

	default:
		break;
	}

	resT.width  = resLR.width;
	resT.height = imageDataLength / 2 / resT.width; //YUYV

    int res = camera->Open(resT.width, resT.height);
	if (res != MS_SUCCESS)
	{
		return res;
	}

	float data[41];
	res = GetCalibParameter(data);
	if (res != MS_SUCCESS)
	{
		return res;
	}

	para.f = data[36];
	para.cu = data[38];
	para.cv = data[39];
	para.b = -data[40];
	float rgb_para[53] = {1};

	string cameraName = GetName();
	string name = "PSP010-400";
	string::size_type idx;
	idx = cameraName.find(name);
	if(idx != string::npos )
	{
		para.f   /= 2 ;
		para.cu  /= 2 ;
		para.cv  /= 2 ;
		data[36] /= 2 ;
		data[38] /= 2 ;
		data[39] /= 2 ;
	}

	algorithm = new Algorithm(data, rgb_para);

	return MS_SUCCESS;
}

void MoveSenseCamera::Close()
{
	delete algorithm;
	algorithm = NULL;
    camera->Close();
}

int MoveSenseCamera::Reset()
{
	return camera->SetRegister(SETTINGS_CMD, DEVICE_HARD_RST);
}

int MoveSenseCamera::StartStream( uint8_t channels/*=CHANNEL_ALL*/)
{
    int res = camera->StartStream();
    if (res != MS_SUCCESS)
    {
        return res;
    }

	camera->SetChannelSwitch(SETTINGS_DIRECTION_ENABLE, true, channels);

	for (int i=0; i<5; i++)
	{
		uint8_t channelNow = MS_CHANNEL_FORWARD << i;

		if ((channels & channelNow) == channelNow)
		{
			res = camera->SetChannelRegister(SETTINGS_SRC_IMG_FPS, setting->framerate, channelNow);
			if (res != MS_SUCCESS)
			{
				return res;
			}

			//TO DO: remove << 4
			res = camera->SetChannelRegister(SETTINGS_MODE, setting->mode<<4, channelNow);
			if (res != MS_SUCCESS)
			{
				return res;
			}
		}
	}

	res = camera->SetRegister(SETTINGS_WORK_START, (uint8_t)WORK_START);
	if (res != MS_SUCCESS)
	{
		return res;
	}

    return camera->WaitForAvailable();
}

int MoveSenseCamera::StopStream()
{
	int res = 0;

	#ifdef _WIN32
    res = camera->StopStream();
	if (res != MS_SUCCESS)
	{
		return res;
	}
	#endif

	camera->SetChannelSwitch(SETTINGS_DIRECTION_ENABLE, false, MS_CHANNEL_ALL);

	res = camera->SetRegister(SETTINGS_WORK_START, (uint8_t)WORK_STOP);
	if (res != MS_SUCCESS)
	{
		return res;
	}

	res = camera->WaitForAvailable();

	#ifndef _WIN32
	if (res != MS_SUCCESS)
	{
		return res;
	}
	res = camera->StopStream();
	#endif
	return res;
}

int MoveSenseCamera::GetImageData(unsigned char *data, int &len, int ms/*=2000*/)
{
	int res = camera->GetImageData(data, imageDataLength, ms);

	if (res == MS_SUCCESS)
	{
		len = imageDataLength;
	}

	return res;
}

int MoveSenseCamera::GetImageData( unsigned char *left, unsigned char *right, unsigned char *disparity, int &len, int ms/*=2000*/ )
{
	int res = GetImageData(imageBuffer, len, ms);

	int w = setting->resolution.width;
	int h = setting->resolution.height;

	int bytesD = setting->disparity.bpp / 8;

	if (res == MS_SUCCESS)
	{
		if (camera->info.parseProtocol == "LINE_ALTER")
		{
			switch (setting->mode)
			{
			case CAMERA_LR:
			case CAMERA_LR_HD:
				for(int i=0; i<h; i++)
				{
					memcpy(left+w*i, (unsigned char*)imageBuffer+i*w*2, w);
					memcpy(right+w*i, (unsigned char*)imageBuffer+i*w*2+w, w);
				}
				break;
			case CAMERA_LD:
			case CAMERA_LD_HD:
				for(int i=0; i<h; i++)
				{
					memcpy(left+w*i, (unsigned char*)imageBuffer+i*w*2, w);
					memcpy(disparity+w*i, (unsigned char*)imageBuffer+i*w*2+w, w);
				}
				break;
			case CAMERA_LRD:
			case CAMERA_LRD_HD:
				for(int i=0; i<h; i++)
				{
					memcpy(left+w*i, (unsigned char*)imageBuffer+i*w*3, w);
					memcpy(right+w*i, (unsigned char*)imageBuffer+i*w*3+w, w);
					memcpy(disparity+w*i, (unsigned char*)imageBuffer+i*w*3+w*2, w);
				}
				break;
			default:
				break;
			}
		}
		else if (camera->info.parseProtocol == "FRAME_ALTER")
		{
			switch (setting->mode)
			{
			case CAMERA_LR:
			case CAMERA_LR_HD:
				memcpy(left, (unsigned char*)imageBuffer, w*h);
				memcpy(right, (unsigned char*)imageBuffer+w*h, w*h);
				break;
			case CAMERA_LD:
			case CAMERA_LD_HD:
				memcpy(left, (unsigned char*)imageBuffer, w*h);
				memcpy(disparity, (unsigned char*)imageBuffer+w*h, w*h);
				break;
			case CAMERA_LRD:
			case CAMERA_LRD_HD:
				memcpy(left, (unsigned char*)imageBuffer, w*h);
				memcpy(right, (unsigned char*)imageBuffer+w*h, w*h);
				memcpy(disparity, (unsigned char*)imageBuffer+w*h*2, w*h);
				break;
			default:
				break;
			}
		}
		else if (camera->info.parseProtocol == "TAG")
		{
			
			int a = 0;
			int b = 0;
			int c = 0;

			switch (setting->mode)
			{
			case CAMERA_LR:
			case CAMERA_LD:
			case CAMERA_LRD:
			case CAMERA_LR_HD:
			case CAMERA_LD_HD:
			case CAMERA_LRD_HD:
				for(int i = 0 ; i < imageDataLength / w / bytesD; i++)
				{
					if(imageBuffer[i*w*bytesD] == 1)
					{
						if(a>=h/bytesD)
						{
							break;
						}
						memcpy(left+w*a*bytesD,	 imageBuffer+(i)*w*bytesD,w*bytesD);
						a++;
					}
					else if(imageBuffer[i*w*bytesD] == 2)
					{
						if(b>=h/bytesD)
						{
							break;
						}
						memcpy(right+w*b*bytesD, imageBuffer+(i)*w*bytesD,w*bytesD);
						b++;
					}
					else if(imageBuffer[i*w*bytesD] == 3)
					{
						if(c>=h)
						{
							break;
						}
						memcpy(disparity+w*c*bytesD, imageBuffer+(i)*w*bytesD,w*bytesD);
						c++;
					}
				}
				break;
			default:
				break;
			}
		}
	}

	return res;
}

int MoveSenseCamera::DisparityToDepth( int disparity )
{
	int depth; //mm
	if (disparity == 0)
	{
		depth = 0;
	}
	else
	{
		int type = setting->disparity.type;
		float disp;
		if (type == 96) //CS480
		{
			if( setting->disparity.bpp == 16 )
			{
				disp = disparity/256.0;
			}
			else
			{
				if (disparity > 128)  disp = (disparity*2-128)/4.0;
				else  disp = disparity/4.0;
			}
		}
		else if (type == 64) //CS240
		{
			disp = disparity/8.0;
		}

		depth = (para.b * para.f)/disp;
	}

	return depth;
}

int MoveSenseCamera::SetStereoRectify( bool onoff, uint8_t channels/*=CHANNEL_ALL*/ )
{
	return camera->SetChannelSwitch(SETTINGS_DIRECTION_RECTIFY_SW, onoff, channels);
}

int MoveSenseCamera::SetCalibParameter(float *para, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	int channelOffset = ChannelOffset(channel);

	int res = camera->SetRegisters(SETTINGS_CALIB_DATA+OFFSET_CHANNEL_CALIB_DATA*channelOffset, (uint8_t*)para, 41*sizeof(float));
	if (res != MS_SUCCESS)
	{
		return res;
	}

	res = camera->SetRegister(SETTINGS_DIRECTION_SET_CALIB_DATA, channel);
	if (res != MS_SUCCESS)
	{
		return res;
	}

	return camera->WaitForAvailable();
}

int MoveSenseCamera::GetCalibParameter(float *para, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	int channelOffset = ChannelOffset(channel);

	return camera->GetRegisters(SETTINGS_CALIB_DATA+OFFSET_CHANNEL_CALIB_DATA*channelOffset, (uint8_t*)para, 41*sizeof(float));
}

int MoveSenseCamera::SetAutoExposure(bool onoff, uint8_t channels/*=CHANNEL_ALL*/)
{
	return camera->SetChannelSwitch(SETTINGS_DIRECTION_AUTO_EXPO, onoff, channels);
}

int MoveSenseCamera::SetAutoGain(bool onoff, uint8_t channels/*=CHANNEL_ALL*/)
{
	return camera->SetChannelSwitch(SETTINGS_DIRECTION_AUTO_GAIN, onoff, channels);
}

int MoveSenseCamera::SetMaxExposure(uint16_t value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->SetRegisters(SETTINGS_MAX_EXPOSURE, (uint8_t*)&value, 2);
}

int MoveSenseCamera::SetMinExposure(uint16_t value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->SetRegisters(SETTINGS_MIN_EXPOSURE, (uint8_t*)&value, 2);
}

int MoveSenseCamera::SetMaxGain(uint16_t value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->SetRegisters(SETTINGS_MAX_GAIN, (uint8_t*)&value, 2);
}

int MoveSenseCamera::SetMinGain(uint16_t value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->SetRegisters(SETTINGS_MIN_GAIN, (uint8_t*)&value, 2);
}

int MoveSenseCamera::GetMaxExposure(uint16_t *value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->GetRegisters(SETTINGS_MAX_EXPOSURE, (uint8_t*)value, 2);
}

int MoveSenseCamera::GetMinExposure(uint16_t *value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->GetRegisters(SETTINGS_MIN_EXPOSURE, (uint8_t*)value, 2);
}

int MoveSenseCamera::GetMaxGain(uint16_t *value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->GetRegisters(SETTINGS_MAX_GAIN, (uint8_t*)value, 2);
}

int MoveSenseCamera::GetMinGain(uint16_t *value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->GetRegisters(SETTINGS_MIN_GAIN, (uint8_t*)value, 2);
}

int MoveSenseCamera::SetDesiredBringhtness(uint8_t value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->SetRegister(SETTINGS_DESIRED_BRIGHTNESS, value);
}

int MoveSenseCamera::GetDesiredBringhtness(uint8_t *value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->GetRegister(SETTINGS_DESIRED_BRIGHTNESS, value);
}

int MoveSenseCamera::SetExposure(uint16_t value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->SetRegisters(SETTINGS_CURRENT_EXPOSURE, (uint8_t*)&value, 2);
}

int MoveSenseCamera::SetGain(uint16_t value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->SetRegisters(SETTINGS_CURRENT_GAIN, (uint8_t*)&value, 2);
}

int MoveSenseCamera::GetExposure(uint16_t *value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->GetRegisters(SETTINGS_CURRENT_EXPOSURE, (uint8_t*)value, 2);
}

int MoveSenseCamera::GetGain(uint16_t *value, uint8_t channel/*=CHANNEL_FORWARD*/)
{
	return camera->GetRegisters(SETTINGS_CURRENT_GAIN, (uint8_t*)value, 2);
}

int MoveSenseCamera::GetSN(uint32_t *sn)
{
	return camera->GetRegisters(SETTINGS_SERIAL_NUM, (uint8_t*)sn, 4);
}

int MoveSenseCamera::SetRegister(uint16_t addr, uint8_t value)
{
	return camera->SetRegister(addr, value);
}

int MoveSenseCamera::GetRegister(uint16_t addr, uint8_t *value)
{
	return camera->GetRegister(addr, value);
}

int MoveSenseCamera::SetRegisters( uint16_t addr, uint8_t *value, int n )
{
	return camera->SetRegisters(addr, value, n);
}

int MoveSenseCamera::GetRegisters( uint16_t addr, uint8_t *value, int n )
{
	return camera->GetRegisters(addr, value, n);
}

void MoveSenseCamera::SpeckleCheck(unsigned short int* src, unsigned short int * dst, int speckleRange /*= 200*/, int cntSize /*= 200 */)
{

	int w = setting->resolution.width;
	int h = setting->resolution.height;
	

	algorithm->SpeckleCheck(src, dst, w, h, speckleRange, cntSize);
}

void MoveSenseCamera::GetPcd(const unsigned short int *src, float *pcd)
{
	int w = setting->resolution.width;
	int h = setting->resolution.height;
	algorithm->GetPcd(src, w, h, pcd);
}

string MoveSenseCamera::GetName()
{
	return camera->info.name;
}

int MoveSenseCamera::GetImageDataLength()
{
	return imageDataLength;
}

CameraSetting MoveSenseCamera::GetSetting()
{
	return camera->info.setting;
}

movesense::CameraPara MoveSenseCamera::GetPara()
{
	return para;
}

Resolution MoveSenseCamera::GetResolution()
{
	return setting->resolution;
}

int MoveSenseCamera::GetFramerate()
{
	return setting->framerate;
}

int MoveSenseCamera::GetMode()
{
	return setting->mode;
}

int MoveSenseCamera::SetResolution( int width, int height )
{
	for (int i=0; i<setting->resolutionList.size(); i++)
	{
		if (width == setting->resolutionList[i].width && height == setting->resolutionList[i].height)
		{
			setting->resolution = setting->resolutionList[i];
			return MS_SUCCESS;
		}
	}
	return MS_FAIL;
}

int MoveSenseCamera::SetFramerate( int framerate )
{
	for (int i=0; i<setting->framerateList.size(); i++)
	{
		if (framerate == setting->framerateList[i])
		{
			setting->framerate = setting->framerateList[i];
			return MS_SUCCESS;
		}
	}
	return MS_FAIL;
}

int MoveSenseCamera::SetMode( int mode )
{
	for (int i=0; i<setting->modeList.size(); i++)
	{
		if (mode == setting->modeList[i])
		{
			setting->mode = setting->modeList[i];
			return MS_SUCCESS;
		}
	}
	return MS_FAIL;
}

int MoveSenseCamera::GetBitDepth()
{
	return setting->disparity.bpp;
}

int MoveSenseCamera::ScanCameras()
{
	return device.ScanCameras();
}


}
